﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Gert3D
{
  public class VoxelGrid
  {
    private Box3 _bounds;
    private double _voxelSize = 0.03;
    private List<Triangle>[,,] _voxels = null;

    public VoxelGrid()
    {
    }

    public void Voxelize(Mesh mesh)
    {
      Box3 gridBounds = new Box3(double.MaxValue, double.MinValue, double.MinValue, double.MaxValue, double.MinValue, double.MaxValue);

      foreach (Triangle tri in mesh.Faces)
      {
        Vector3 pa = tri.VertexA.Position;
        Vector3 pb = tri.VertexB.Position;
        Vector3 pc = tri.VertexC.Position;

        gridBounds.Left = Math.Min(gridBounds.Left, Math.Min(pa.X, Math.Min(pb.X, pc.X)));
        gridBounds.Right = Math.Max(gridBounds.Right, Math.Max(pa.X, Math.Max(pb.X, pc.X)));

        gridBounds.Bottom = Math.Min(gridBounds.Bottom, Math.Min(pa.Y, Math.Min(pb.Y, pc.Y)));
        gridBounds.Top = Math.Max(gridBounds.Top, Math.Max(pa.Y, Math.Max(pb.Y, pc.Y)));

        gridBounds.Front = Math.Min(gridBounds.Front, Math.Min(pa.Z, Math.Min(pb.Z, pc.Z)));
        gridBounds.Back = Math.Max(gridBounds.Back, Math.Max(pa.Z, Math.Max(pb.Z, pc.Z)));
      }

      _bounds = gridBounds;

      int voxelsX = (int)((gridBounds.Width()) / _voxelSize) + 1;
      int voxelsY = (int)((gridBounds.Height()) / _voxelSize) + 1;
      int voxelsZ = (int)((gridBounds.Depth()) / _voxelSize) + 1;

      _voxels = new List<Triangle>[voxelsX, voxelsY, voxelsZ];

      Box3 voxel;
      for (int z = 0; z < voxelsZ; z++)
      {
        voxel.Front = gridBounds.Front + z * _voxelSize;
        voxel.Back = (voxel.Front + _voxelSize);
        for (int y = 0; y < voxelsY; y++)
        {
          voxel.Bottom = gridBounds.Bottom + y * _voxelSize;
          voxel.Top = (voxel.Bottom + _voxelSize);
          for (int x = 0; x < voxelsX; x++)
          {
            voxel.Left = gridBounds.Left + x * _voxelSize;
            voxel.Right = (voxel.Left + _voxelSize);

            foreach (Triangle tri in mesh.Faces)
            {
              //Check bounding box of triangle
              if (voxel.Intersects(tri.Bounds))
              {
                List<Triangle> voxelTris = null;
                if (_voxels[x, y, z] == null)
                {
                  voxelTris = new List<Triangle>();
                  _voxels[x, y, z] = voxelTris;
                }
                else
                  voxelTris = _voxels[x, y, z];

                voxelTris.Add(tri);
              }

              //If tri is not sorted out, check triangle sides for intersection with voxel simple
              //If still not sorted out, check triangle sides with box surfaces
              //If still not sorted out, check voxel sides with triangle surface
            }            
          }
        }
      }
    }

    public IntersectionInfo IntersectRay(Ray ray)
    {
      int vx = 0;
      int vy = 0;
      int vz = 0;

      Vector3 rayOrgLocal = new Vector3();

      if (_bounds.IsInside(ray.Origin))
      {
        rayOrgLocal.X = ray.Origin.X - _bounds.Left;
        rayOrgLocal.Y = ray.Origin.Y - _bounds.Bottom;
        rayOrgLocal.Z = ray.Origin.Z - _bounds.Front;
      }
      else
      {
        IntersectionInfo info = ray.IntersectBox(_bounds, double.MaxValue);
        if (info != null)
        {
          rayOrgLocal.X = info.IntersectionPoint.X - _bounds.Left;
          rayOrgLocal.Y = info.IntersectionPoint.Y - _bounds.Bottom;
          rayOrgLocal.Z = info.IntersectionPoint.Z - _bounds.Front;
        }
        else
          return null; //Ray does not hit voxelspace
      }

      vx = (int)(rayOrgLocal.X / _voxelSize);
      vy = (int)(rayOrgLocal.Y / _voxelSize);
      vz = (int)(rayOrgLocal.Z / _voxelSize);

      int stepX = Math.Sign(ray.Direction.X);
      int stepY = Math.Sign(ray.Direction.Y);
      int stepZ = Math.Sign(ray.Direction.Z);

      int outX = stepX < 0.0 ? -1 : (int)((_bounds.Width()) / _voxelSize) + 1;
      int outY = stepY < 0.0 ? -1 : (int)((_bounds.Height()) / _voxelSize) + 1;
      int outZ = stepZ < 0.0 ? -1 : (int)((_bounds.Depth()) / _voxelSize) + 1;

      double tMaxX = ray.Direction.X != 0.0 ? (((vx + (stepX < 0 ? 0 : 1)) * _voxelSize) - rayOrgLocal.X) / ray.Direction.X : double.PositiveInfinity;
      double tMaxY = ray.Direction.Y != 0.0 ? (((vy + (stepY < 0 ? 0 : 1)) * _voxelSize) - rayOrgLocal.Y) / ray.Direction.Y : double.PositiveInfinity;
      double tMaxZ = ray.Direction.Z != 0.0 ? (((vz + (stepZ < 0 ? 0 : 1)) * _voxelSize) - rayOrgLocal.Z) / ray.Direction.Z : double.PositiveInfinity;

      double tDeltaX = ray.Direction.X != 0.0 ? Math.Abs(_voxelSize / ray.Direction.X) : 0.0;
      double tDeltaY = ray.Direction.Y != 0.0 ? Math.Abs(_voxelSize / ray.Direction.Y) : 0.0;
      double tDeltaZ = ray.Direction.Z != 0.0 ? Math.Abs(_voxelSize / ray.Direction.Z) : 0.0;

      IntersectionInfo result = null;

      List<Triangle> tris = null;
      do
      {
        tris = _voxels[vx, vy, vz];

        if (tris != null)
        {
          double minT = double.MaxValue;
          foreach (Triangle tri in tris)
          {
            IntersectionInfo info = ray.IntersectTriangle(tri, minT);
            if (info != null)
            {
              minT = info.T;
              result = info;
            }
          }

          if (result != null)
          {
            int ix = (int)((result.IntersectionPoint.X - _bounds.Left) / _voxelSize);
            int iy = (int)((result.IntersectionPoint.Y - _bounds.Bottom) / _voxelSize);
            int iz = (int)((result.IntersectionPoint.Z - _bounds.Front) / _voxelSize);

            if (ix == vx && iy == vy && iz == vz)
              return result;
          }
        }

        if (tMaxX < tMaxY)
        {
          if (tMaxX < tMaxZ)
          {
            vx = vx + stepX;
            if (vx == outX)
              return null;
            tMaxX = tMaxX + tDeltaX;
          }
          else
          {
            vz = vz + stepZ;
            if (vz == outZ)
              return null;
            tMaxZ = tMaxZ + tDeltaZ;
          }
        }
        else
        {
          if (tMaxY < tMaxZ)
          {
            vy = vy + stepY;
            if (vy == outY)
              return null;
            tMaxY = tMaxY + tDeltaY;
          }
          else
          {
            vz = vz + stepZ;
            if (vz == outZ)
              return null;
            tMaxZ = tMaxZ + tDeltaZ;
          }
        }          
      } while (true);

        /*double minT = double.MaxValue;
        foreach (Triangle tri in tris)
        {
          IntersectionInfo info = ray.IntersectTriangle(tri, minT);
          if (info != null)
          {
            minT = info.T;
            result = info;
          }
        }

        if (result != null)
        {
          int ix = (int)((result.IntersectionPoint.X - _bounds.Left) / _voxelSize);
          int iy = (int)((result.IntersectionPoint.Y - _bounds.Bottom) / _voxelSize);
          int iz = (int)((result.IntersectionPoint.Z - _bounds.Front) / _voxelSize);
          
          if (ix != vx || iy != vy || iz != vz)
            result = null;
        }*/

      //return result;
    }

    public double VoxelSize
    {
      get { return _voxelSize; }
      set { _voxelSize = value; }
    }

    public List<Triangle> this[int x, int y, int z]
    {
      get { return _voxels[x, y, z]; }
      set { _voxels[x, y, z] = value; }
    }

    public Box3 Bounds
    {
      get { return _bounds; }
      set { _bounds = value; }
    }

  }
}
